using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX.DirectInput;

namespace JoystickControl {
    class Program {
        static Device joystick = null;
        static Blimpbots.Control.Blimp blimp;
        static void Main(string[] args) {

            if (args.Length != 1) {
                Console.WriteLine("args: blimp_port");
                blimp = new Blimpbots.Control.Blimp("com43", "blimp");
            }
            else {
                blimp = new Blimpbots.Control.Blimp(args[0], "blimp");
            }

            // partly based on 
            // http://msdn2.microsoft.com/en-us/library/bb153252.aspx

            foreach (
                DeviceInstance di in
                Manager.GetDevices(
                    DeviceClass.GameControl,
                    EnumDevicesFlags.AttachedOnly)) {
                joystick = new Device(di.InstanceGuid);
                break;
            }

            if (joystick == null) {
                //Throw exception if joystick not found.
                throw new Exception("No joystick found.");
            }

            joystick.Properties.AxisModeAbsolute = true;
        
            joystick.Acquire();
            while (true) {
                System.Threading.Thread.Sleep(100);
                updateJoystick();
            }

        }

        private static void updateJoystick() {
            string info = "Joystick: ";

            JoystickState state = joystick.CurrentJoystickState;

            //Capture Position.
            info += "X:" + state.X + " ";
            info += "Y:" + state.Y + " ";
            //info += "Z:" + state.Z + " ";
            /*int[] extraAxis = state.GetSlider();
            for (int i = 0; i < extraAxis.Length; i++) {
                info += "a" + i + ": " + extraAxis[i] + " ";
            }*/

            //Capture Buttons.
            /*byte[] buttons = state.GetButtons();
            for (int i = 0; i < buttons.Length; i++) {
                if (buttons[i] != 0) {
                    info += "Button:" + i + " ";
                }
            }*/

            /*int[] pov = state.GetPointOfView();
            for (int i = 0; i < pov.Length; i++) {
                info += "p" + i + ": " + pov[i] + " ";
            }*/

            double maxMotor = 7.5;

            double reverse = ((double)state.Y - 32768) / 32768;

            if (reverse > 1 || reverse < -1) {
                throw new Exception("bad joystick read");
            }

            double forward = 0;
            if (reverse < 0) {
                forward = -1 * reverse;
                reverse = 0;
            }

            double right = ((double)state.X - 32768) / 32768;
            double left = 0;
            if (right < 0) {
                left = -1 * right;
                right = 0;
            }

            double leftMotor = 0;
            double rightMotor = 0;

            if (forward > 0 && right > 0) {
                leftMotor = max(forward * maxMotor, right*maxMotor);
                rightMotor = forward * maxMotor - right * maxMotor;
                Console.WriteLine("forward right");
            }
            else if (forward > 0) {
                // forward left
                rightMotor = max(forward * maxMotor, left*maxMotor);
                leftMotor = forward * maxMotor - left * maxMotor;
                Console.WriteLine("forward left");
            } 
            // now we must be in a reverse case
            else if (right > 0) {
                rightMotor = -1 * max(reverse * maxMotor, right * maxMotor);
                leftMotor = (1 - reverse) * maxMotor - (1 - right) * maxMotor;
                Console.WriteLine("reverse right");
            }
            else {
                // reverse left
                leftMotor = -1 * max(reverse * maxMotor, left*maxMotor);
                rightMotor = (1 - reverse) * maxMotor - (1 - left) * maxMotor;
                Console.WriteLine("reverse left");
            }

            if (rightMotor > maxMotor || leftMotor > maxMotor) {
                throw new Exception("bad math.");
            }

            double pov = state.GetPointOfView()[0];
            double zMotor = 0;
            if (pov == -1) {
                zMotor = 0;
            }
            else if (pov == 0) {
                zMotor = maxMotor;
            }
            else if (pov == 31500 || pov == 4500) {
                zMotor = .5 * maxMotor;
            }
            else if (pov == 22500 || pov == 13500) {
                zMotor = -.5 * maxMotor;
            }
            else if (pov == 18000) {
                zMotor = -1 * maxMotor;
            }

            // LEDs
            byte[] buttons = state.GetButtons();

            blimp.SetLEDs((buttons[0] > 0), (buttons[1] > 0));

            blimp.setFPSpeed(1, leftMotor);
            blimp.setFPSpeed(2, zMotor);
            blimp.setFPSpeed(3, rightMotor);

            if((ticks++ % 20) == 0) blimp.requestVoltage();

            info += " motors: L " + leftMotor + " R " + rightMotor + " z " + zMotor;

            Console.WriteLine(info);
            Console.WriteLine("V={0:f2}", blimp.Voltage);
        }

        static int ticks = 0;

        private static double max(double a, double b) {
            if (a > b) return a;
            return b;
        }
    }
}
